function [X, Y]=findControlSequence(tour, original, d)
% X are n control points that is in the range of XX-d<=X<=XX+d
% Y are in-between control points, n-1
% three dimension, minimize length
% use cvx for optimization

N=size(tour,1);
XX=tour';
XX=XX(:);

cvx_begin
    variable Y(3*(N-1))
    variable X(3*N);
    variable S1(3*(N-1))
    variable S2(3*(N-1))
    minimize (norm(S1)+norm(S2))
    subject to
        S1==X(1:3*(N-1))-Y;
        S2==Y-X(4:3*N);
        
        for i=1:N
            if original(i)
                X((3*i-2):(3*i))<=XX((3*i-2):(3*i))+d;
                X((3*i-2):(3*i))>=XX((3*i-2):(3*i))-d;                
            else
                X(3*i-2)==XX(3*i-2);
                X(3*i-1)==XX(3*i-1);
                X(3*i)>=XX(3*i);
            end
        end
%         X(test_z)>=XX(test_z);
%         X<=XX+d;
%         X>=XX-d;
        2*X(4:3*(N-1))==Y(1:3*(N-2))+Y(4:3*(N-1));
cvx_end
